#include "node_lateral3dis.h"
#include "ros/ros.h"
#include <yaml-cpp/yaml.h>
#include "data_2d2s_path.h"

int main(int argc, char **argv)
{
	std::string node_yaml_file_name = data_2d2s_path + "node_lateral.yaml";
	YAML::Node node_yaml = YAML::LoadFile(node_yaml_file_name);
	double rate = node_yaml["loop_rate"].as<double>();
	ros::init(argc, argv, "osqp_control");
	ROS_INFO("INIT SUSCESS");
	auto_ros::control::NodeLateral3dis node(rate);
	ROS_INFO("NODE INIT");
	while (1)
	{
		node.LoopProc();
		ros::spinOnce(); //可用回调函数
		node.loop_rate_.sleep();
	}

	return 0;
}